home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Language/OS - Multiplatform Resource Library
/
LANGUAGE OS.iso
/
cpp_libs
/
cool
/
ge_cool.lha
/
GE_COOL2.1
/
src
/
Transform
/
Quaternion.h
< prev
Wrap
C/C++ Source or Header
|
1992-07-14
|
6KB
|
185 lines
//
// Copyright (C) 1992 General Electric Company.
//
// Permission is granted to any individual or institution to use, copy, modify,
// and distribute this software, provided that this complete copyright and
// permission notice is maintained, intact, in all copies and supporting
// documentation.
//
// General Electric Company,
// provides this software "as is" without express or implied warranty.
//
// Created: VDN 06/23/92 -- design and implementation
//
// Quaternion is a vector with 1 real and 3 imaginary parts:
// q = cos(theta/2) + sin(theta/2) (x i + y j + z k)
// Quaternion is represented by a 4-elmt vector, with imaginary parts
// being the first 3 elements, so that they are aligned with x, y, z, and
// the real part, the fourth element.
//
// References:
// 1. Horn, B.K.P. (1987) Closed-form solution of absolute orientation using
// unit quaternions. J. Opt. Soc. Am. Vol 4, No 4, April.
// 2. Horn, B.K.P. (1987) Robot Vision. MIT Press. pp. 437-551.
#ifndef QUATERNIONH
#ifndef MATRIXH
#include <cool/Matrix.h>
DECLARE CoolMatrix<float>; // need only forward decl.
#endif
#ifndef M_VECTORH
#include <cool/M_Vector.h>
DECLARE CoolM_Vector<float>; // include parent class
#endif
class CoolQuaternionE; // forward decl of envelope
class CoolQuaternion : public CoolM_Vector<float> {
public:
CoolQuaternion (float x = 0, float y = 0, float z = 0, // default is null quat.
float r = 1.0);
CoolQuaternion (const CoolM_Vector<float>& axis, // from axis&angle
float angle);
CoolQuaternion (const CoolMatrix<float>& transform); // from 2-4 square row-major
inline CoolQuaternion (const CoolM_Vector<float>& vec); // from 2-4D vector
inline CoolQuaternion (const CoolQuaternion& q); // copy constructor
inline ~CoolQuaternion(); // free internal array
inline float& x (); // imaginary component
inline float& y (); // parallel to axis of rotation
inline float& z ();
inline float& r (); // real component
inline float real () const;
inline CoolM_Vector<float>E imaginary () const; // imaginary vector part
CoolM_Vector<float>E axis () const; // Axis of rotation
float angle () const; // Angle of rotation
inline CoolQuaternion& operator= (const CoolQuaternion& rhs); // q1 = q2
inline CoolQuaternion& operator= (CoolQuaternionE& env); // q1 = env
Boolean operator== (const CoolQuaternion& rhs) const; // cmp with fuzz = 1.0e-6
inline Boolean operator!= (const CoolQuaternion& rhs) const; // instead of 1.0e-8
inline operator CoolM_Vector<float>& (); // cast to 4-elmt vector
CoolMatrix<float>E rotation_transform (int dim = 4) const; // to 2-4 rot matrix
CoolQuaternionE conjugate () const; // same real, opposite img part
CoolQuaternionE inverse () const; // inverse for nonzero quat
friend CoolQuaternionE operator* (const CoolQuaternion& q1, // q = q1 * q2 * q3
const CoolQuaternion& q2);
void rotate (CoolM_Vector<float>& v) const; // rotate 3D v, store result in v.
inline friend ostream& operator<< (ostream& os, const CoolQuaternion& q);
inline friend ostream& operator<< (ostream& os, const CoolQuaternion* q);
};
#define CoolEnvelope CoolQuaternionE
#define CoolLetter CoolQuaternion
#include <cool/Envelope.h> // use Envelope to avoid deep
#undef CoolEnvelope // copies on return by value
#undef CoolLetter
// Quaternion -- Construct Quaternion from 2-, 3- or 4-elmt vector.
// If nD-vector, construct imaginary Quaternion.
// Input: 2D, 3D location vector, or 4D Quaternion vector.
inline CoolQuaternion::CoolQuaternion (const CoolM_Vector<float>& vec)
: CoolM_Vector<float>(vec) { // 1-1 layout between vector&quat
}
// Quaternion -- Copy constructor
// Input: Quaternion
inline CoolQuaternion::CoolQuaternion (const CoolQuaternion& q)
: CoolM_Vector<float>(q) { // 1-1 layout between vector&quat
}
// ~Quaternion -- Nothing, since Quaternion has same data as Vector.
inline CoolQuaternion::~CoolQuaternion () {} // Vector will free data
// x -- Access first imaginary component, along x axis.
inline float& CoolQuaternion::x () {
return this->data[0];
}
// y -- Access second imaginary component, along y axis.
inline float& CoolQuaternion::y () {
return this->data[1];
}
// z -- Access third imaginary component, along z axis.
inline float& CoolQuaternion::z () {
return this->data[2];
}
// r -- Access real component
inline float& CoolQuaternion::r () {
return this->data[3];
}
// real -- Get real part
inline float CoolQuaternion::real () const {
return this->data[3];
}
// imaginary -- Get imaginary part
inline CoolM_Vector<float>E CoolQuaternion::imaginary () const {
return this->extract(3,0);
}
// operator= -- Assignment q1 = q2;
// Input: Quaternion on rhs
// Output: Quaternion on lhs, with contents of Quaternion on rhs copied over.
inline CoolQuaternion& CoolQuaternion::operator= (const CoolQuaternion& rhs) {
CoolM_Vector<float>::operator=(rhs); // same as copy vector part
return *this;
}
// operator= -- Assignment from an envelope back to real Quaternion
// Swap the contents over, rhs envelope will be deleted by compiler.
inline CoolQuaternion& CoolQuaternion::operator= (CoolQuaternionE& env) {
env.shallow_swap((CoolQuaternionE*)this, &env); // same physical layout
return *this;
}
// operator!= -- Components of Quaternion are compared with fuzz = 1.0e-6
inline Boolean CoolQuaternion::operator!= (const CoolQuaternion& rhs) const {
return (!operator==(rhs));
}
// operator M_Vector -- Automatic conversion to a 4-elmt vector.
// Input: *this, a Quaternion.
// Output: a reference to the 4-elmt vector, through a type-cast.
inline CoolQuaternion::operator CoolM_Vector<float>& () {
return *((CoolM_Vector<float>*) this); // same physical space
}
// operator<< -- Print the components of Quaternion.
inline ostream& operator<< (ostream& os, const CoolQuaternion& q) {
return os << *((CoolM_Vector<float>*) &q);
}
inline ostream& operator<< (ostream& os, const CoolQuaternion* q) {
return os << *((CoolM_Vector<float>*) q);
}
#endif // QuaternionH